Unified Multi-Modal Landmark Tracking for Tightly Coupled Lidar-Visual-Inertial Odometry

We present an efficient multi-sensor odometry system for mobile platforms that jointly optimizes visual, lidar, and inertial information within a single integrated factor graph. This runs in real-time at full framerate using fixed lag smoothing. To perform such tight integration, new method to extract 3D line planar primitives from lidar point clouds is presented. approach overcomes the suboptimality of typical frame-to-frame tracking methods by treating as landmarks them over multiple scans. True integration features with standard visual IMU made possible subtle passive synchronization camera frames. The lightweight formulation allows execution on CPU. Our proposed has been tested variety scenarios, including underground exploration legged robot outdoor scanning dynamically moving handheld device, total duration 96 min 2.4 km traveled distance. In these test sequences, only one exteroceptive sensor leads failure due either underconstrained geometry (affecting lidar) or textureless areas caused aggressive lighting changes vision). conditions, our graph naturally uses best available each modality without any hard switches.